﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using AMStudio.Common;
using System.Text.RegularExpressions;
using Microsoft.Practices.EnterpriseLibrary.ExceptionHandling;
using Microsoft.Practices.EnterpriseLibrary.Common.Configuration;
using Microsoft.Practices.EnterpriseLibrary.Logging;
using AMStudio.DelegateDispather;
using System.IO.Ports;
using System.Threading;
using System.Collections;

namespace AMStudio.Driver
{
    public class RCXRobotDriver : IAutomationDriver
    {
        protected ExceptionManager ExManager = EnterpriseLibraryContainer.Current.GetInstance<ExceptionManager>();

        private DelegateDispather.IEventClient eventClient;
        public DelegateDispather.IEventClient EventClient
        {
            get { return eventClient; }
            set { eventClient = value; }
        }

        private RobotConfig config = null;
        private RobotPoint[] points = null;
        private SerialPort hardwaredriver;
        private bool alreadyinitialized = false;
        private IExecuteReader result = new ExecuteReader(false);

        private String hardwareName = "ST7_ROBOT";
        public String HardwareName
        {
            get { return hardwareName; }
            set { hardwareName = value; }
        }
        private String hardwareEvent = "ST7_ROBOT_RET";

        public String HardwareEvent
        {
            get { return hardwareEvent; }
            set { hardwareEvent = value; }
        }
        public virtual int ExecuteNonQuery(string command, params object[] parameters)
        {
            return -1;
        }

        private object syncRoot = new object();
        private Queue<object[]> ExecuteReaderQueue = new Queue<object[]>();
        private string Command = string.Empty;
        private int intBeginRead = 0;
        private uint intPointNu = 0;
        private bool CommEnd = true;

        public virtual IExecuteReader ExecuteReader(string command, params object[] parameters)
        {
            lock (syncRoot)
            {
                result.Success = false;

                try
                {
                    switch (command)
                    {
                        case "INIT":
                            {
                                #region "INIT"

                                if (alreadyinitialized)
                                {
                                    result.Success = true;
                                    result.Result = true;
                                    break;
                                }
                                IEventManager em = parameters[0] as IEventManager;
                                eventClient = new EventClient(em);
                                config = parameters[1] as RobotConfig;
                                this.hardwareEvent = config.HardwareEvent;
                                this.hardwareName = config.HardwareName;
                                Logger.Write("Start to init hardware " + hardwareName);
                                hardwaredriver = new SerialPort();
                                hardwaredriver.PortName = config.SerialPort.PortName;
                                hardwaredriver.BaudRate = config.SerialPort.BandRate;
                                hardwaredriver.Parity = config.SerialPort.Parity;
                                hardwaredriver.DataBits = config.SerialPort.DataBits;
                                hardwaredriver.StopBits = config.SerialPort.StopBits;
                                hardwaredriver.Handshake = config.SerialPort.HandShake;
                                hardwaredriver.DtrEnable = true;
                                hardwaredriver.RtsEnable = true;
                                hardwaredriver.WriteTimeout = config.SerialPort.WriteTimeout;
                                hardwaredriver.ReadTimeout = config.SerialPort.ReadTimeout;
                                hardwaredriver.DataReceived += new SerialDataReceivedEventHandler(hardwaredriver_DataReceived);
                                hardwaredriver.Open();

                                RaiseEvent("CONNECT", true, null);

                                Command = string.Empty;
                                result.Success = true;
                                result.Result = true;
                                alreadyinitialized = true;
                                #endregion
                            }
                            break;

                        case "EXIT":
                            {
                                #region "EXIT"

                                if (hardwaredriver != null)
                                {
                                    hardwaredriver.Close();
                                    hardwaredriver = null;
                                }

                                alreadyinitialized = false;
                                RaiseEvent("DISCONNECT", true, null);

                                if (Command != string.Empty)
                                {
                                    result.Success = true;
                                    result.Result = new object[] { false, "连接丢失!" };
                                    RaiseEvent(Command, true, result);
                                    Command = string.Empty;
                                }

                                if (Command == string.Empty)
                                {
                                    DequeueCommand();
                                }

                                #endregion
                            }
                            break;

                        default:
                            ExecuteReaderQueue.Enqueue(new object[] { command, parameters });

                            if (Command != string.Empty)
                            {
                                if (!alreadyinitialized)
                                {
                                    result.Success = true;
                                    result.Result = new object[] { false, "连接丢失!" };
                                    RaiseEvent(Command, true, result);
                                    Command = string.Empty;
                                }
                            }

                            if (Command != string.Empty && !CommEnd)
                            {
                                int intEndRead = Environment.TickCount & Int32.MaxValue;
                                int intTimeout = intEndRead < intBeginRead ? int.MaxValue - intBeginRead + intEndRead : intEndRead - intBeginRead;

                                if (intTimeout > config.SerialPort.ReadTimeout)
                                {
                                    result.Success = true;
                                    result.Result = new object[] { false, "通讯超时!" };
                                    RaiseEvent(Command, true, result);
                                    Command = string.Empty;
                                }
                            }

                            if (Command == string.Empty)
                            {
                                DequeueCommand();
                            }
                            break;

                        case "CONNECT_STATUS":
                            {
                                #region "CONNECT_STATUS"

                                if (alreadyinitialized && hardwaredriver.IsOpen)
                                {
                                    result.Success = true;
                                    result.Result = true;
                                }
                                else
                                {
                                    result.Success = false;
                                    result.Result = false;
                                }
                                RaiseEvent("CONNECT_STATUS", true, result.Result);
                                #endregion
                            }
                            break;
                    }
                }
                catch (Exception ex)
                {
                    ExManager.HandleException(ex, "Policy");
                    alreadyinitialized = false;
                    RaiseEvent("DISCONNECT", true, null);
                    if (hardwaredriver != null)
                        hardwaredriver.Close();
                }
                return result;
            }
        }

        private string receivedData = string.Empty;
        private string allNotes = string.Empty;
        void hardwaredriver_DataReceived(object sender, SerialDataReceivedEventArgs e)
        {
            try
            {
                receivedData += hardwaredriver.ReadExisting();

                if (receivedData.IndexOf("\r\n") == -1)
                {
                    return;
                }

                switch (Command)
                {
                    case "CHECK_VERSION":
                        #region "CHECK_VERSION"
                        result.Result = new object[] { true, receivedData.Substring(0, receivedData.IndexOf(",")).Trim() };
                        receivedData = string.Empty;
                        break;
                        #endregion

                    case "GET_SERVOSTATUS":
                        #region "GET_SERVOSTATUS"
                        result.Result = new object[] { true, receivedData.Trim() };
                        receivedData = string.Empty;
                        break;
                        #endregion

                    case "GET_EMGSTATUS":
                        #region "GET_EMGSTATUS"
                        result.Result = new object[] { true, int.Parse(receivedData.Trim()) };
                        break;
                        #endregion

                    case "GET_ALLNOTES":
                        #region "GET_ALLNOTES"
                        if (receivedData.IndexOf("\r\n") == 0)
                        {
                            result.Result = new object[] { true, new Dictionary<uint, string>() };
                        }
                        else
                        {
                            if (receivedData.IndexOf("\r\n\r\n") == -1)
                            {
                                return;
                            }

                            Dictionary<uint, string> notes = new Dictionary<uint, string>();
                            string[] mutiStr = receivedData.Split(new string[] { "\r\n" }, System.StringSplitOptions.RemoveEmptyEntries);

                            for (int k = 0; k < mutiStr.Length; k++)
                            {
                                int _id = mutiStr[k].IndexOf('=');

                                if (_id == -1 || _id < 3)
                                {
                                    continue;
                                }

                                uint point = uint.Parse(mutiStr[k].Substring(2, _id - 2));
                                string note = mutiStr[k].Substring(_id + 1);

                                if (notes.ContainsKey(point))
                                {
                                    notes[point] = note;
                                }
                                else
                                {
                                    notes.Add(point, note);
                                }
                            }

                            result.Result = new object[] { true, notes };
                        }
                        break;
                        #endregion

                    case "GET_ASSIGNEDPOINTS":
                        #region "GET_ASSIGNEDPOINTS"
                        if (receivedData.Contains(":"))
                        {
                            result.Result = new object[] { false, receivedData };
                        }
                        else
                        {
                            string[] txp = receivedData.Split(new char[] { ' ' }, System.StringSplitOptions.RemoveEmptyEntries);
                            RobotPoint p = new RobotPoint();

                            if (txp.Length >= 6)
                            {
                                p.Number = intPointNu;
                                p.X = Convert.ToSingle(txp[0]);
                                p.Y = Convert.ToSingle(txp[1]);
                                p.Z = Convert.ToSingle(txp[2]);
                                p.R = Convert.ToSingle(txp[3]);
                                p.A = Convert.ToSingle(txp[4]);
                                p.B = Convert.ToSingle(txp[5]);
                                p.ArmType = Convert.ToInt32(txp[6]);
                                p.Note = string.Empty;
                                result.Result = new object[] { true, p };
                            }
                            else
                            {
                                result.Result = new object[] { false, receivedData };
                            }
                        }
                        break;
                        #endregion

                    case "GET_ALLPOINTS":
                        #region "GET_ALLNOTES"
                        if (receivedData.IndexOf("\r\n") == 0)
                        {
                            result.Result = new object[] { true, new Dictionary<uint, RobotPoint>() };
                        }
                        else
                        {
                            if (receivedData.IndexOf("\r\n\r\n") == -1)
                            {
                                return;
                            }

                            Dictionary<uint, RobotPoint> points = new Dictionary<uint, RobotPoint>();
                            string[] mutiStr = receivedData.Split(new string[] { "\r\n" }, System.StringSplitOptions.RemoveEmptyEntries);

                            for (int k = 0; k < mutiStr.Length; k++)
                            {
                                string[] str = mutiStr[k].Split(new char[] { '=' });

                                if (str.Length != 2)
                                {
                                    continue;
                                }

                                RobotPoint point = new RobotPoint();
                                point.Number = uint.Parse(str[0].Substring(1).Trim());

                                string[] str2 = str[1].Split(new char[] { ' ' }, StringSplitOptions.RemoveEmptyEntries);

                                if (str2.Length < 6)
                                {
                                    continue;
                                }

                                point.X = Convert.ToSingle(str2[0].Trim());
                                point.Y = Convert.ToSingle(str2[1].Trim());
                                point.Z = Convert.ToSingle(str2[2].Trim());
                                point.R = Convert.ToSingle(str2[3].Trim());
                                point.A = Convert.ToSingle(str2[4].Trim());
                                point.B = Convert.ToSingle(str2[5].Trim());
                                point.Note = string.Empty;
                                point.ArmType = str2.Length > 6 ? int.Parse(str2[6].Trim()) : 0;

                                if (points.ContainsKey(point.Number))
                                {
                                    points[point.Number] = point;
                                }
                                else
                                {
                                    points.Add(point.Number, point);
                                }
                            }

                            result.Result = new object[] { true, points };
                        }
                        break;
                        #endregion

                    case "WRITE_POINTS":
                        #region "WRITE_POINTS"
                        if (!receivedData.Contains("Please enter !"))
                        {
                            result.Result = new object[] { false, receivedData.Trim() };
                        }
                        else
                        {
                            Command = "WRITE_POINTS_SEND_DATA";
                            intBeginRead = Environment.TickCount & Int32.MaxValue;
                            receivedData = string.Empty;

                            string cmdline = string.Empty;

                            foreach (RobotPoint p in points)
                            {
                                cmdline += "P" + p.Number.ToString() + "=" + p.X.ToString("f2") + " " + p.Y.ToString("f2") + " " + p.Z.ToString("f2") + " " + p.R.ToString("f2") + " " + p.A.ToString("f2") + " " + p.B.ToString("f2") + " " + p.ArmType.ToString() + config.SerialPort.ETX;
                            }

                            cmdline += config.SerialPort.ETX;
                            hardwaredriver.Write(cmdline);
                            return;
                        }
                        break;
                        #endregion

                    case "WRITE_POINTS_SEND_DATA":
                    case "CHANGE_RUNMODE":
                    case "SET_SERVOSTATUS":
                    case "RSET_EMGSTATUS":
                    case "TEACH_POINT":
                    case "SET_POINT":
                    case "SET_UNIT":
                        #region "WRITE_POINTS_SEND_DATA,CHANGE_RUNMODE,SET_SERVOSTATUS,RSET_EMGSTATUS,TEACH_POINT,SET_POINT"
                        if (!receivedData.Contains("OK"))
                        {
                            result.Result = new object[] { false, receivedData.Trim() };
                        }
                        else
                        {
                            result.Result = new object[] { true };
                        }
                        break;
                        #endregion

                    case "GET_CURRENT":
                        {
                            #region "GET_CURRENT"
                            RobotPoint p = new RobotPoint();
                            string[] multstr = receivedData.Split(new char[] { '\r', '\n', ' ' }, StringSplitOptions.RemoveEmptyEntries);

                            if (multstr.Length < 7)
                            {
                                result.Result = new object[] { false, receivedData.Trim() };
                            }
                            else
                            {
                                p.X = Convert.ToSingle(multstr[1]);
                                p.Y = Convert.ToSingle(multstr[2]);
                                p.Z = Convert.ToSingle(multstr[3]);
                                p.R = Convert.ToSingle(multstr[4]);
                                p.A = Convert.ToSingle(multstr[5]);
                                p.B = Convert.ToSingle(multstr[6]);
                                p.ArmType = 0;
                                p.Note = string.Empty;
                                result.Result = new object[] { true, p };
                            }
                            #endregion
                        }
                        break;
                }

                CommEnd = true;
                result.Success = true;
                RaiseEvent(Command, true, result);
                DequeueCommand();
            }
            catch (TimeoutException)
            {
            }
        }

        private void DequeueCommand()
        {
            lock (syncRoot)
            {
                Command = string.Empty;

                bool loop = true;

                while (loop && ExecuteReaderQueue.Count != 0)
                {
                    object[] obj = ExecuteReaderQueue.Dequeue();

                    if (obj.Length != 2)
                    {
                        continue;
                    }

                    if (!alreadyinitialized)
                    {
                        result.Success = true;
                        result.Result = new object[] { false, "未初始化!" };
                        RaiseEvent(obj[0].ToString(), true, result);
                        continue;
                    }

                    switch (obj[0].ToString())
                    {
                        case "CHECK_VERSION":
                            #region "CHECK_VERSION"
                            Command = obj[0].ToString();
                            intBeginRead = Environment.TickCount & Int32.MaxValue;
                            receivedData = string.Empty;
                            CommEnd = false;
                            hardwaredriver.Write("@?VER" + config.SerialPort.ETX);
                            loop = false;
                            break;
                            #endregion
                        case "SET_UNIT":
                            #region "SET_UNIT"
                            Command = obj[0].ToString();
                            intBeginRead = Environment.TickCount & Int32.MaxValue;
                            receivedData = string.Empty;
                            CommEnd = false;
                            hardwaredriver.Write("@UNIT 1" + config.SerialPort.ETX);
                            loop = false;
                            break;
                            #endregion
                        case "CHANGE_RUNMODE":
                            {
                                #region "CHANGE_RUNMODE"
                                object[] args = (object[])obj[1];
                                string[] cmd = new string[4] { "AUTO", "PROGRAM", "MANUAL", "SYSTEM" };
                                Command = obj[0].ToString();
                                intBeginRead = Environment.TickCount & Int32.MaxValue;
                                receivedData = string.Empty;
                                CommEnd = false;
                                hardwaredriver.Write("@" + cmd[int.Parse(args[0].ToString())] + config.SerialPort.ETX);
                                loop = false;
                                break;
                                #endregion
                            }

                        case "GET_SERVOSTATUS":
                            #region "GET_SERVOSTATUS"
                            Command = obj[0].ToString();
                            intBeginRead = Environment.TickCount & Int32.MaxValue;
                            receivedData = string.Empty;
                            CommEnd = false;
                            hardwaredriver.Write("@?SERVO" + config.SerialPort.ETX);
                            loop = false;
                            break;
                            #endregion

                        case "SET_SERVOSTATUS":
                            {
                                #region "SET_SERVOSTATUS"
                                object[] args = (object[])obj[1];
                                Command = obj[0].ToString();
                                intBeginRead = Environment.TickCount & Int32.MaxValue;
                                receivedData = string.Empty;
                                CommEnd = false;
                                hardwaredriver.Write("@SERVO " + ((object[])args[0])[0].ToString() + " " + ((object[])args[0])[1].ToString() + config.SerialPort.ETX);
                                loop = false;
                                break;
                                #endregion
                            }

                        case "GET_EMGSTATUS":
                            #region "GET_EMGSTATUS"
                            Command = obj[0].ToString();
                            intBeginRead = Environment.TickCount & Int32.MaxValue;
                            receivedData = string.Empty;
                            CommEnd = false;
                            hardwaredriver.Write("@?EMG" + config.SerialPort.ETX);
                            loop = false;
                            break;
                            #endregion

                        case "RSET_EMGSTATUS":
                            #region "RSET_EMGSTATUS"
                            Command = obj[0].ToString();
                            intBeginRead = Environment.TickCount & Int32.MaxValue;
                            receivedData = string.Empty;
                            CommEnd = false;
                            hardwaredriver.Write("@EMGRST" + config.SerialPort.ETX);
                            loop = false;
                            break;
                            #endregion

                        case "GET_ALLNOTES":
                            #region "GET_ALLNOTES"
                            Command = obj[0].ToString();
                            intBeginRead = Environment.TickCount & Int32.MaxValue;
                            receivedData = string.Empty;
                            CommEnd = false;
                            hardwaredriver.Write("@READ PCM" + config.SerialPort.ETX);
                            loop = false;
                            break;
                            #endregion

                        case "GET_ASSIGNEDPOINTS":
                            #region "GET_ASSIGNEDPOINTS"
                            {
                                object[] args = (object[])obj[1];
                                Command = obj[0].ToString();
                                intPointNu = uint.Parse(args[0].ToString());
                                intBeginRead = Environment.TickCount & Int32.MaxValue;
                                receivedData = string.Empty;
                                CommEnd = false;
                                hardwaredriver.Write("@P" + intPointNu.ToString() + config.SerialPort.ETX);
                                loop = false;
                                break;
                            }
                            #endregion

                        case "GET_ALLPOINTS":
                            #region "GET_ALLPOINTS"
                            Command = obj[0].ToString();
                            intBeginRead = Environment.TickCount & Int32.MaxValue;
                            receivedData = string.Empty;
                            CommEnd = false;
                            hardwaredriver.Write("@READ PNT" + config.SerialPort.ETX);
                            loop = false;
                            break;
                            #endregion

                        case "TEACH_POINT":
                            #region "TEACH_POINT"
                            {
                                object[] args = (object[])obj[1];
                                Command = obj[0].ToString();
                                intPointNu = uint.Parse(args[0].ToString());
                                intBeginRead = Environment.TickCount & Int32.MaxValue;
                                receivedData = string.Empty;
                                CommEnd = false;
                                hardwaredriver.Write("@TEACH" + intPointNu.ToString() + config.SerialPort.ETX);
                                loop = false;
                                break;
                            }

                            #endregion

                        case "SET_POINT":
                            {
                                #region "SET_POINT"
                                object[] args = (object[])obj[1];
                                RobotPoint p = (RobotPoint)args[0];
                                Command = obj[0].ToString();
                                intBeginRead = Environment.TickCount & Int32.MaxValue;
                                receivedData = string.Empty;
                                CommEnd = false;
                                hardwaredriver.Write("@P" + p.Number.ToString() + "=" + p.X.ToString("f2") + " " + p.Y.ToString("f2") + " " + p.Z.ToString("f2") + " " + p.R.ToString("f2") + " " + p.A.ToString("f2") + " " + p.B.ToString("f2") + " " + p.ArmType.ToString() + config.SerialPort.ETX);
                                loop = false;
                                break;
                                #endregion
                            }

                        case "WRITE_POINTS":
                            {
                                #region "WRITE_POINTS"
                                object[] args = (object[])obj[1];
                                points = (RobotPoint[])args[0];
                                Command = obj[0].ToString();
                                intBeginRead = Environment.TickCount & Int32.MaxValue;
                                receivedData = string.Empty;
                                CommEnd = false;
                                hardwaredriver.Write("@WRITE PNT" + config.SerialPort.ETX);
                                loop = false;
                                break;
                                #endregion
                            }

                        case "GET_CURRENT":
                            #region "GET_CURRENT"
                            Command = obj[0].ToString();
                            intBeginRead = Environment.TickCount & Int32.MaxValue;
                            receivedData = string.Empty;
                            CommEnd = false;
                            hardwaredriver.Write("@?WHRXY" + config.SerialPort.ETX);
                            loop = false;
                            break;
                            #endregion

                    }
                }
            }
        }

        public void RaiseEvent(string command, bool status, object value)
        {
            if (command == "WRITE_POINTS_SEND_DATA")
            {
                command = "WRITE_POINTS";
            }

            StationEventArgs arg = new StationEventArgs(hardwareName, command, status, value);
            eventClient.RaiseEvent(hardwareEvent, arg);
        }
    }
}
